/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file axisgroup.h
 *  功能:
 * 多轴对象的管理和对外接口。
 *
 *                                                                         *
 ***************************************************************************/
#ifndef AxisGROUP_H
#define AxisGROUP_H
#include "axisobject.h"
#include <qvector.h>
#include "JointDecouple/jointdecouple.h"
#include "coordinatetrajectory.h"
#include "coordinatemanager.h"
#include "zeromanagement.h"

#define ZERO_CONFIG_FILE_NAME "/config/zeroConfig.xml"

struct GroupTrajeAttribute
{
    bool isJointTraj;
    double timeDuration_math;
    int cnt;
    int lineId;
    double timeDuration_norminal;
    double startCutoffTime;
    double endCutoffTime;

};

/**************************************
//单位统一说明：
//关节采用　s , 　mm ,　degree。
代码的计算与单位无关，只要单位统一，结果就是正确的。
**************************************/
/**
 * @brief The AxisGroup class 运动轴组控制
 */
class AxisGroup
{
public:
    AxisGroup(int robotIdIn,RobotCommonParameter * roboParameterIn,
              QVector<JointAttribute> axisGroupAttribute,
              QVector<JointAttribute>  axisGroupCoordinateAttributeIn
              , int groupId, double deltaTimeIn, int robotTypeIn, MotionMessage *motionMessageIn,
              CoordinateManager* coordinateManagerIn);

public:
    int axisSimpleLoopOnce();
    int getAxisSimpleTpInfo(int axisId,SimpleTpInfo &infoOut);
    int controlAxisSimpleTp(int axisId,int isEnable);
    int setAxisSimpleTpTarget(int axisId,double targetPosIn,double maxVelIn,double maxAccIn);
    E_AXIS_PLAN_MODE getAxisPlanMode(int axisId);
    int setAxisPlanMode(int axisId,E_AXIS_PLAN_MODE modeIn);

    int getIdealTorque(QVector<double>& torqueOut);
    int setIdealTorque(QVector<double> torqueIn);
    bool isInDecartTrajStatus();
    int getErrorStatusFlag();
    int clearAllMsg( );
    int setRobotCoordinateConfig(struct RobotCoordinateAxisConfig coordinateConfigIn);
    float getRunningNumber();
    int setRunningNumber_robotArmControl(float numberIn);
    double getMixDurationLeft();
    int setRotateEqradius(double rotate_eqradiusIn);
    int ceaseAxisGroup();
    double getAxisGroupStopTime();
    double getAxisGroupVelLimitRatio();
    int setAxisProperty(int axisId, struct AxisProperty_hmi axisPropertyIn);
    int setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn, double approachDistanceIn);
    int setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn);
    int setAccelerationRatio(double ratioIn);
    int setAccelerationRatio_RobotArmJoint(double ratioIn);
    int setAccelerationRatio_RobotArmLine(double ratioIn);
    int setRobotArmLineMaxAcceleration(double accelerationIn);
    int setRobotArmLineMaxVelocity(double velIn);
    int setRobotArmJointMaxAcceleration(int axisIdIn,double accelerationIn);
    int setRobotArmJointMaxVelocity(int axisIdIn, double velIn);
    E_ZERO_TASK_MODE getZeroTaskMode();
    int getMotorEncoderPulsePerRound(QVector<double>& pulseOut);
    int setServoOnStatus(const QVector<bool> &servoOnStatus);
    int setServoOnStatus(int axisId, bool isOn);
    int axisFeedbackCheck();
    int isPositionCommandApprochLimit();
    //for agv only
    int setAgvAxisVel(double velIn ,int agvAxisId);//agv轴不存在的话就不生效。 
    int setAgvAxisVel(QVector<double> velIn );
    int setAgvSteeringPosition(double positionIn ,int agvAxisId);
    int setAgvSteeringPosition(QVector<double> positionIn);
    int getAgvSteeringPositionFeedback(QVector<double> &positionOut);
    //
    int calibrateJointByAiEncoder(int axisId, double aiValue);
    bool isAbsoluteMotor(int axisId);
    int startGoZero(int axisId);
    int startGoZero();
    int haltGoZero();
    int getGoZeroAxis(int &axisId);
    int changeToNextGoZeroAxis();
    int getZeroMovingType(EM_ZERO_MOVINT_TYPE &zeroMovingType);

    int zeroManagementLoopRealTime();
    int zeroManagementLoopCommonTime();
    int getZeroReturnParameter(QVector<ZeroReturnParameter> &parameterOut);
    int getDebugInfo(AxisGroupDebugInfo& infoOut);
    int restoreControlMode(int axisId);
    int restoreControlMode();
    int getMotorControlMode(QVector<E_MOTOR_CONTROL_MODE> &controlModeOut);
    int getMotorControlMode(int axisId,E_MOTOR_CONTROL_MODE &modeOut);
    int setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn);
    int setMotorControlMode(E_MOTOR_CONTROL_MODE modeIn);
    /**
     * @brief setTimeAxisScale　设置时间轴缩放比例
     * @param ratio　缩放比例０～１
     * @param timeDuration　多长时间到达设定比例
     * @return
     */
    int setTimeAxisScale(double ratio, double timeDuration, int timeAxisIndex);
    int setTimeAxisScaleByTrap(double ratio,int timeAxisIndex, int acceleration);
    int setTimeAxisScaleBySpline(double ratio,int timeAxisIndex);
    int setAllTimeAxisScale(double ratio, double timeDuration);

    int setNewTimeAxisVelLimitRatio_coordAxis(double scaleIn);
    int restoreTimeAxisVelLimitRatio_coordAxis(double scaleIn);
    int resetTimeAxisVelLimitRatio_coordAxis();
    /**
     * @brief setDeltaTime　设置时间步长
     * @param time
     * @return
     */
    int setDeltaTime(double time);
    /**
     * @brief createAxisObject　创建轴对象
     * @param axisId
     * @param axisAttribute
     * @return
     */
    int createAxisObject(int axisId, JointAttribute axisAttribute);

    int createCoordinateAxisObject(int axisId, double deltaTimeIn, JointAttribute axisAttribute,double rotate_eqradius);

    double getLineRemainingTime();
    /**
     * @brief moveOneStep 计算一步，通过内部时间轴取点
     * @return 　０没轨迹或超极限　１有轨迹
     */
    int moveOneStep();
    int moveOneStep_cnt();
    int updateCntCutoffTime();
    int isTargetArrived();
    int moveOneStep_byWorldCoordinate(const Frame &worldFrameIn,bool isOverLay=false,double durationLeft=0);
    int moveOneStep_byUserCoordinateCommand(const PointPro &pointIn);
    int getCoordinateAxisPositionCommand(Frame &positionFrameCommandOut);//获取坐标轴命令值
    int getFinishedAxisPositionCommand(Frame &positionFrameCommandOut);//获取坐标轴或关节轴命令值
    int transeferCoordinateAxisData2JointAxisData();
    int timeAxisMoveOneStep(QVector<double> &timeAxisScaleOut);

    /**
     * @brief executeDataToMotor　解耦并转换单位
     * @return　-1　失败，机器人类型不支持或轴状态错误　　１成功
     */
    int executeDataToMotor();

    /**
     * @brief isAllVelProfileFinished　判断所有轨迹是否走完
     * @param axisIdVector　需要判断的axisId
     * @return　０至少有１条轨迹　１无轨迹
     */
    int isAllVelProfileFinished(QVector<int> axisIdVector);
    int isAllCoordinateVelProfileFinished(QVector<int> axisIdVector);
    int isAllVelProfileFinished( );
    int isTargetOffsetCheckOk();
    Frame getPositionFrameCommand();
    int setFrameFeedback(QVector<Frame> frameList);
//    int setFrameCommand(QVector<Frame> frameList);
    int setFrameCommand_one(Frame frameIn);
    int setPositionFrameCommand_byJointCommand();
    int setPositionFrameCommandPlaned_byJointCommandPlaned();
    int setJointCommandPlaned_byPositionFrameCommandPlaned();
    int calculateCurrentFrameCommandPoint_byJointCommand(Frame &positionFrameCommandOut);
    int calculateCurrentFrameCommandPoint_byJointCommand(PointPro &positionCommandOut);
    /**
     * @brief upDateAllFeedbackAndOffset 更新各级反馈值，并计算FollowOffset
     * @return -1　失败，机器人类型不支持　　１成功
     */
    int upDateAllFeedbackAndOffset();

    int getJointFeedback(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                         QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut );
    int getJointCommand(QVector<double> &jointsPositionOut);
    int getJointCommand_planed(QVector<double> &jointsPositionOut);

    int getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                    QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut );

    double getAccumulatedTime(int trajIndexIn=0);
    double getAccumulatedTime_joint(int trajIndexIn=0);
    double getAccumulatedTime_decart(int trajIndexIn=0);
    int setVelCompensation(double velIn,int axisId );//速度补偿
    int setTorqueCompensation(double torqueIn,int axisId );//力矩补偿
    int setPlanStartPosition(double positionIn,int axisId);
    int setJointPlanStartPosition_byPositionCommand();

    /**
     * @brief moveOneStepByPosition 直接给一个目标点
     * @param positionIn
     * @param axisId
     * @return ０不成功，前面的轨迹未完成　　１成功
     */
    int moveOneStepByPosition(double positionIn,int axisId);//axid从０开始
    int moveOneStepByPosition_plus(double positionPlus,int axisId);
    int moveOneStepByPosition_prepareMix(double positionIn,double durationLeftIn,int axisId);
    int moveOneStepByPosition_overlay(double positionIn,int axisId);
    int moveOneStepByPosition_withMixer(double positionIn1, double positionIn2,
                                        double mixRatioIn, int axisId);
    int moveOneStepByPosition_vel(double velIn,int axisId);//axid从０开始
    int moveOneStepByPV(double positionIn, double velIn ,int axisId);
    int moveOneStepByV(double velIn,int axisId );
    int moveOneStepByV_filter(double velIn ,int axisId);
    int moveOneStepByPVAT(double positionIn, double velIn, double accIn, double torqueIn,int axisId);
    int moveOneStepByTorque(double torqueIn,int axisId);

    /**
     * @brief asyncTrapMove 梯形运动，//异步到达。起始和终止点速度为０。保证限定　速度／加速度　不超过最大限制
     * @param positionIn　终止点
     * @param speedPercent　最大速度百分比
     * @param axisIdVector 所需要控制的轴id组，axid从０开始
     * @return 　０不成功，前面的至少１个轴的轨迹未完成　　１成功
     */
    int asyncTrapMove(QVector<double> positionVector, double speedPercent, QVector<int> axisIdVector);


    int lineMove(Frame positionIn, double speedPercent, double &timeDurationResult,
                 int axisId, double minMixDurationIn, int cntIn, int idIn);

    int lineContinuousMove_mixedCNT(QVector<Frame> positionIn, QVector<double> speedPercent,
                                    QVector<double> &timeDurationResult, QVector<double> cntList,
                                    int axisId, double minMixDurationIn, QVector<int> accListIn);
    int lineContinuousMove(QVector<Frame> positionIn, QVector<double> speedPercent,
                           QVector<double> &timeDurationResult, double cnt, int axisId);

    int circleMove(QVector<Frame>  positionIn, double speedPercent, QVector<double> &timeDurationResult, int axisId, int cntIn, int idIn);




    /**
     * @brief asyncTrapMove 梯形运动，//同步到达。起始和终止点速度为０。保证限定　速度／加速度　不超过最大限制
     * @param positionIn　终止点
     * @param timeDuration　运动过程时间。实际可能比设定要长
     * @param axisIdVector 所需要控制的轴id组，axid从０开始
     * @return 　０不成功，前面的至少１个轴的轨迹未完成　　１成功
     */
    int syncTrapMoveByTime(QVector<double> positionVector, double timeDuration,
                           QVector<int> axisIdVector, double speedRatio);
    /**
     * @brief asyncSplineZeroMove　 样条曲线运动, //异步到达。起始和终止点速度/加速度为０。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionVector
     * @param speedPercent
     * @param axisIdVector
     * @return
     */
    int asyncSplineZeroMove(QVector<double> positionVector, double speedPercent, QVector<int> axisIdVector);
    /**
     * @brief syncSplineZeroMove　样条曲线运动, //同步到达。起始和终止点速度/加速度为０。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionVector
     * @param speedPercent
     * @param axisIdVector
     * @return
     */
    int syncSplineZeroMove(bool isImprovedSpline,QVector<double> positionVector, double speedPercent,
                           QVector<int> axisIdVector, double &timeDurationResult);//not used
    int syncZeroMove_velVariable(E_VELOCITY_TYPE velocityStyleIn, QVector<double> positionVector,
                       double speedPercent, QVector<int> axisIdVector, double &timeDurationResult,
                                 double minTimeDurationIn, int cntIn,int idIn);
    /**
     * @brief asyncTrapMove 梯形运动，//同步到达。起始和终止点速度为０。保证限定　速度／加速度　不超过最大限制
     * @param positionIn　终止点
     * @param speedPercent　最大速度百分比
     * @param axisIdVector 所需要控制的轴id组，axid从０开始
     * @return 　０不成功，前面的至少１个轴的轨迹未完成　　１成功
     */
    int syncTrapMove(QVector<double> positionVector, double speedPercent, QVector<int> axisIdVector,
                     double &timeDurationResult);
    /**
     * @brief syncSplineZeroMoveByTime　样条曲线运动, //同步到达。起始和终止点速度/加速度为０。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * @param positionVector
     * @param timeDuration
     * @param axisIdVector
     * @return
     */
    int syncSplineZeroMoveByTime(QVector<double> positionVector,double timeDuration, QVector<int> axisIdVector);
    /**
     * @brief syncSplineContinueMove　样条曲线连续运动。目标位置／速度／加速度为参数值。
     * 保证限定　速度　不超过最大限制。加速度可能超过。kdl不支持全段限制速度、加速度。
     * @param positionVector
     * @param velVector
     * @param accVector
     * @param axisIdVector
     * @return
     */
    int syncSplineContinueMove(QVector<double> positionVector,QVector<double> velVector
                               ,QVector<double> accVector,QVector<int> axisIdVector);//同步到达

    int splineContinueMoveMultiPoint(QVector<QVector<VelPoint> > velPointArry, QVector<int> axisIdVector,
                                     int isLastPointMove, QVector<double> &timeDurationListResult);//多点通过

    /**
     * @brief syncSplineContinueMoveByTime　样条曲线连续运动。目标位置／速度／加速度为参数值。
     * 速度　加速度可能超过最大限制。。kdl不支持全段限制速度、加速度。
     * 1)
     * @param positionVector
     * @param velVector
     * @param accVector
     * @param timeDuration
     * @param axisIdVector
     * @return
     */
    int syncSplineContinueMoveByTime(QVector<double> positionVector ,QVector<double> velVector ,
                                     QVector<double> accVector,double timeDuration,QVector<int> axisIdVector);//同步到达
//    /**
//     * @brief cease　使轨迹点减速暂停，时间轴为零时才退出。轨迹还存在。
//     * @return
//     */
//    int cease();
//    /**
//     * @brief stop　使轨迹点减速停止后，从新复位所有轴，旧轨迹不存在。
//     * @return
//     */
//    int stop();
    /**
     * @brief reset　清除所有轴的旧轨迹，并更新反馈值到命令值。
     * @return
     */
    int clearTraj(bool isSyncFeedbackPos);//时间轴不变
    int clearFrontTraj();
    int clearFrontTraj_joint();
    int clearFrontTraj_decart();
    int addTrajInfo(bool isJointTraj, double timeDuration_math, int cnt, int lineId);

    int clearTraj_byAxisId(int axisIndex, bool isSyncFeedbackPos);
    int resetCoordinateTraj(int axisIndex);

    int resetAll();//时间轴也复位

//    int syncCommandAndFeedback();

    int writeDataToFile();

    double getTimeAxisScale(int timeAxisIndex);
    double getTimeAxisScale();

    QVector<MotorData> getMotorCommandDataVector();
    QVector<MotorData> getMotorFeedbackDataVector();
    int setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn);
    bool isInFineMoving();//是否为fine精确运动
    int isTimeVelProfileEmpty(int timeAxisIndex);
    int isTimeVelProfileEmpty();
//    int setAxisPositionCommand(std::vector<float> positionValue);

    int setCheckMode(E_CHECK_MODE modeIn);
    int getFeedbackJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque);

    int validateZeroData();
    int axisLosePower();
    int axisLosePower(int axisId);
    int calibrateRobotByOffset(std::vector<double> offsetIn);
    int calibrateRobot();
    int calibrateRobot(int axisId);
    bool isAxisZeroOk();
    bool isAxisZeroOk(int axisId);
    int getAxisZeroState(int axisId);
    QVector<int> getAbsoluteZeroMotorValue();
    QVector<double> getJointMechanicalZeroValue();
    int getCalibrateInformation(struct S_AXISGROUP_ZERO_INFORMATION& zeroInformationOut);

    int addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int _robotId,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);

private:
    int initialTimeAxis();//初始时间轴
    bool isPositonValueChanged(QVector<double> positionVector,QVector<int> axisIdVector);





public:
    QVector<AxisObject*> axisArry;//轴对象数组
    QVector<CoordinateTrajectory*> coordinateAxisArry;//坐标系轴对象数组(求解器没有分多个，导致不能支持多坐标轴，其余功能都可以）
    JointDecouple* axisGroupDecouple;//解耦器

private:
    int robotType;//机器人类型
    int robotId;
    Frame finishedPositionFrameCommand;//最终有效点。

    QQueue<GroupTrajeAttribute>  groupTrajQueue;
    QVector<JointAttribute>  axisGroupAttribute;//轴组属性
    QVector<JointAttribute>  axisGroupCoordinateAttribute;//轴组属性
    QVector<AxisObject*> timeAxis;//时间轴组
    int axisGroupId;//轴组编号，０开始
    int totalAxisNumber;//轴总数
    int armControlAxisNumber;//机械手臂控制轴
//    int externAxisNumber;//外部轴数量
    int agvAxisNum;
    int virtualAxisNum;
    int coordinateAxisNumber;
    double deltaTime;//单步时间长
    int basicStatus;//基本状态
    int currentRunningLine;//当前运行行
    int iscurrentAxisCommandFInished;//当前运行行是否结束
//    double currentCommandRemainingTime;//当前运行行剩余时间
    double totalRemainingTime;//总剩余时间
    double ceaseTimeDuration;//暂停动作时间
    MotionMessage *motionMessage;
    CoordinateManager* coordinateManager;
    ZeroManagement* zeroManagement;

    AxisGroupDebugInfo debugInfo;
    bool isInCntPlan;
    double currentLineRemainingTime;
    volatile float currentLine; //0.2=算法已取点, 0.5=正在运行，0.9=(已经到目标位置),  ,下一行+0.2(避免精度丢失,不用0.1)
    int exceedTimeCount;
    double targetWaitTime;
    int errorStatusFlag;
    bool isInDecartTraj;

};

#endif // AxisGROUP_H
